#ifndef _FORMATION_BSPLINE_TRAJECTORY_OPTIMIZE_H
#define _FORMATION_BSPLINE_TRAJECTORY_OPTIMIZE_H

#include <iostream>
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <nlopt.hpp>
#include <unordered_set>
#include <queue>
#include "bspline_cius/non_uniform_bspline_cius.h"
#include "minimum_snap_path_plan/minimum_snap_path_plan.h"
#include "matplotlibcpp.h"
#include "trajectory_formation/backward.hpp"
#include <fstream>
#include <sstream>
#include <gmm_voxel_map/gmm_voxel_map.h>
#include "formation_avoidance/utils.h"
#include "formation_avoidance/formation_structure.h"

class FormationBsplineTrajectoryOptimize
{
public:
	/* data */
	gvm::GMMVoxelMapCircularBuffer* bsplineOptimizeMapPtr;
	int agent_num;
	std::vector<std::vector<Eigen::Vector3i>> agent_idxList;
	double long_edge=2.0;
	double short_edge=1.0;
	double long_short_ratio=long_edge/short_edge;
	double resolution=0.1;
	double SAFETY_DISTANCE=0.5;
	double max_vel_=0.1;
	double max_acc_=2.0;
	double ts=0.25;
	std::vector<trajectory_formation::NonUniformBspline> init;
	int optimize_iter_times=0;
	std::vector<double> cost_record;
	std::vector<std::vector<Eigen::Vector3d>> optimize_ctrl_pts;
	int agent_id;
	double control_frequency;
	int color_id;
	double setFlightHeight=0.1;
	bool endPointSoftConstraint=true;
	int variable_num;
	int x_each_piece_num;
	bool HeightConstraint=false;
	bool velocityDirectionConstraint=true;
	std::vector<Eigen::Vector3d> trajectroyOptimize_current_p;
	double plan_end_v=0;

	double lamda_Smoothness=20.123;
	double lamda_Distance=20.0;
	double lamda_Feasibility=0.01;
	double lamda_Collision=0.1;
	double lamda_HeightLimitation=50;
	double lamda_EndPoint=0.1;
	double lamda_velocityDirectionCost=0.0;
	double lamda_SecondHalfCollisionPunishRatio=1.5;
	double lamda_BaseFormation=0.1;

	double lamda_Formation=0.1;

	double SmoothnessCost=0;
	double DistanceCost=0;
	double FeasibilityCost=0;
	double CollisionCost=0;
	double HeightLimitationCost=0;
	double EndPointCost=0;
	double velocityDirectionCost=0;

	double FormationCost=0;
	double BaseFormationCost=0;

	double SmoothnessCost_Sum=0;
	double DistanceCost_Sum=0;
	double FeasibilityCost_Sum=0;
	double EndPointCost_Sum=0;
	double velocityDirectionCost_Sum=0;
	double CollisionCost_Sum=0;
	int optimize_round=0;

	std::vector<std::pair<int,std::vector<double>>> bsplineBasisValueAndStartIdx;

	double bsplineDurationTime;	


	int FORMATION_TYPE;
	formation_avoidance::Formation_Structure formation_structure_info;

	double formation_edge_ratio;
	double formation_safety_distance;
	double formation_base_edge_length;


	int ctrl_num;
public:

	FormationBsplineTrajectoryOptimize(){}
	~FormationBsplineTrajectoryOptimize(){}

	void LinkMap(gvm::GMMVoxelMapCircularBuffer* gvmMap,double set_safety_distance, double set_max_vel, double set_max_acc,int set_agent_id,double set_rate)
	{
		bsplineOptimizeMapPtr= gvmMap;
		resolution=gvmMap->resolution_;
		SAFETY_DISTANCE=set_safety_distance;
		max_vel_=set_max_vel;
		max_acc_=set_max_acc;
		agent_id=set_agent_id;
		control_frequency=set_rate;
		color_id=agent_id+32;
		ts=0.25;

		formation_structure_info.formation_structure_init(formation_avoidance::formation( FORMATION_TYPE),formation_edge_ratio,
															formation_safety_distance,formation_base_edge_length);
		agent_num=formation_structure_info.formation_agent_num;

		init.resize(agent_num);
		agent_idxList.resize(agent_num);
		optimize_ctrl_pts.resize(agent_num);
		trajectroyOptimize_current_p.resize(agent_num);
	}

	std::vector<Eigen::Vector3i> SetInitPath(std::vector<Eigen::Vector3d> current_p,Eigen::Vector3d local_goal, std::vector<Eigen::Vector3i> idxList)
	{
		const std::vector<Eigen::Vector3i> tmp=idxList;
		std::vector<Eigen::Vector3i> offset=formation_structure_info.calculateCentreOffset();
		
		double end_heading;
		if(formation_structure_info.formation_type==formation_avoidance::Diamond)
			end_heading=std::atan2(current_p[0](1)-current_p[1](1),current_p[0](0)-current_p[1](0));
		if(formation_structure_info.formation_type==formation_avoidance::Rectangle)
			end_heading=std::atan2(current_p[3](1)-current_p[0](1),current_p[3](0)-current_p[0](0));
		if(formation_structure_info.formation_type==formation_avoidance::Vertical_Line)
			end_heading=std::atan2(current_p[0](1)-current_p[3](1),current_p[0](0)-current_p[3](0));

			
		ROS_INFO("ID: %d end_heading: %f",agent_id,end_heading);
		Eigen::Matrix3d tmp_trans;
		tmp_trans<<cos(end_heading),-std::sin(end_heading),0,
					sin(end_heading),cos(end_heading),0,
					0,0,1;
		for(int i=0;i<agent_num;i++)
		{
			offset[i]=(tmp_trans*offset[i].cast<double>()).cast<int>();
			if(agent_id==0)
				ROS_INFO("id: %d i: %d'sOFFSET  %d %d %d",agent_id,i,offset[i](0),offset[i](1),offset[i](2));
		}

		for(int id=0;id<agent_num;id++)
		{
			agent_idxList[id].clear();

			Eigen::Vector3i start_idx;
			bsplineOptimizeMapPtr->getmapIdxLocal(current_p[id],start_idx);

			agent_idxList[id].push_back( start_idx);
			if(tmp.size()>=3)
			{
				for(int i=0;i<tmp.size()-1;i++)
				{
					if(((tmp[i]-tmp[i+1]).cast<double>()).norm()*bsplineOptimizeMapPtr->resolution_  >0.7)
					{
						double ratio=1.0;
						Eigen::Vector3i insert_point=((1-ratio)*tmp[i].cast<double>()+ratio*tmp[i+1].cast<double>()).cast<int>()+offset[id];
						
						if(insert_point(0)<agent_idxList[id][0](0))
							continue;
						
						agent_idxList[id].push_back(insert_point );
						for(int j=i+2;j<tmp.size();j++)
						{
							agent_idxList[id].push_back(tmp[j]+offset[id]);
						}
						break;
					}
					if(i==tmp.size()-2)
					{
						// std::cout<<"can't find tmp i "<<std::endl;
						double ratio=0.9;
						agent_idxList[id].push_back( ((1-ratio)*tmp[i].cast<double>()+ratio*tmp[i+1].cast<double>()).cast<int>()+offset[id]);						for(int j=2;j<tmp.size();j++)
						for(int j=i+1;j<tmp.size();j++)
						{
							agent_idxList[id].push_back(tmp[j]+offset[id]);
						}
						break;					
					}
				}
			}
			else
			{
				agent_idxList[id].push_back( tmp.back()+offset[id]);
			}
			// agent_idxList[id].back()+=(offset[id]).cast<int>();
			
			// Eigen::Vector3i tmp=(offset[id]).cast<int>();
			// ROS_INFO("id: %d offset: %d %d %d",id,tmp(0),tmp(1),tmp(2));

			std::vector<Eigen::Vector3i> check_idx;
			for(int i=0;i<(int)agent_idxList[id].size()-1;i++)
			{
				if(agent_idxList[id][i]!=agent_idxList[id][i+1])
					check_idx.push_back(agent_idxList[id][i]);
			}
			check_idx.push_back(agent_idxList[id].back());
			agent_idxList[id]=check_idx;

		}
		
		if(agent_id==0)
		{ 
			std::cout<<"a star: //-----"<<std::endl;
			for(int i=0;i<tmp.size();i++)
			{
				std::cout<<tmp[i](0)<<", "<<tmp[i](1)<<", "<<tmp[i](2)<<", "<<std::endl;
			}

			for(int id=0;id<agent_num;id++)
			{
				std::cout<<"after change: id: "<<id<<std::endl;
				for(int i=0;i<agent_idxList[id].size();i++)
				{
					std::cout<<agent_idxList[id][i](0)<<", "<<agent_idxList[id][i](1)<<", "<<agent_idxList[id][i](2)<<", "<<std::endl;
				}
			}
		}

		return agent_idxList[agent_id];


	}

	void getControlPointsFormOptimizationVariable(std::vector<double> x, std::vector<std::vector<Eigen::Vector3d>>& q)
	{
		int ctrl_p_num=init[0].getControlPointNum();

		for(int id=0;id<agent_num;id++)
		{
			q[id].resize(ctrl_p_num);
			q[id][0]=init[id].control_points_.row(0);
			q[id][1]=init[id].control_points_.row(1);
			q[id][2]=init[id].control_points_.row(2);

			if(!endPointSoftConstraint)
			{
				for(int i=0;i<=ctrl_p_num-7;i++)
				{
					q[id][i+3]<<x[i*3+0+id*x_each_piece_num],x[i*3+1+id*x_each_piece_num],x[i*3+2+id*x_each_piece_num];
				}
				q[id][ctrl_p_num-3]=init[id].control_points_.row(ctrl_p_num-3);
				q[id][ctrl_p_num-2]=init[id].control_points_.row(ctrl_p_num-2);
				q[id][ctrl_p_num-1]=init[id].control_points_.row(ctrl_p_num-1);
			}
			else
			{
				for(int i=0;i<=ctrl_p_num-4;i++)
				{
					q[id][i+3]<<x[i*3+0+id*x_each_piece_num],x[i*3+1+id*x_each_piece_num],x[i*3+2+id*x_each_piece_num];
				}

			}

			// std::cout<<"id: "<<id<<" ctrl_pts: \n"<<ctrl_pts[id]<<std::endl;
		}

		// for(int id=2;id<agent_num;id++)
		// {
		// 	q[id].resize(ctrl_p_num);
		// 	q[id][0]=init[id].control_points_.row(0);
		// 	q[id][1]=init[id].control_points_.row(1);
		// 	q[id][2]=init[id].control_points_.row(2);
		// }

		// for(int i=3;i<ctrl_p_num;i++)
		// {
		// 	Eigen::Vector3d pos0=q[0][i];
		// 	Eigen::Vector3d pos1=q[1][i];
		// 	formation_structure_info.set_p0_p1(pos0,pos1);
		// 	std::vector<Eigen::Vector3d>	pos=		formation_structure_info.calculateAllPosition();

		// 	for(int id=2;id<agent_num;id++)
		// 	{
		// 		q[id][i]=pos[id];
		// 	}
			
		// }

	}

	void calcSmoothnessCost(const std::vector<Eigen::Vector3d>& q, double& cost,std::vector<Eigen::Vector3d>& gradient) 
	{
		int order=3;
		cost = 0.0;
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient.begin(), gradient.end(), zero);
		Eigen::Vector3d jerk, temp_j;

		for (int i = 0; i <(int) q.size() - order; i++) {
			/* evaluate jerk */
			jerk = q[i + 3] - 3 * q[i + 2] + 3 * q[i + 1] - q[i];
			cost += jerk.squaredNorm();
			temp_j = 2.0 * jerk;
			/* jerk gradient */
			gradient[i + 0] += -temp_j;
			gradient[i + 1] += 3.0 * temp_j;
			gradient[i + 2] += -3.0 * temp_j;
			gradient[i + 3] += temp_j;
		}
	}

	void calcDistanceCost(const std::vector<Eigen::Vector3d>& q, double& cost,std::vector<Eigen::Vector3d>& gradient)
	{
		int order=3;
		cost = 0.0;
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient.begin(), gradient.end(), zero);

		double          dist=0;
		Eigen::Vector3d dist_grad, g_zero(0, 0, 0);
		int end_i;
		end_i=endPointSoftConstraint ? q.size():(q.size() - order);
		int start_i=end_i*0.3;
		for (int i = start_i; i <end_i; i++) 
		{
			bsplineOptimizeMapPtr->evaluateEDTWithGrad(q[i], -1.0, dist, dist_grad);
			if (dist_grad.norm() > 1e-4) dist_grad.normalize();
			dist_grad(2)=0;
			// ROS_INFO("dist: %f",dist);
			double D=SAFETY_DISTANCE;
			if (dist < D) 
			{
				cost += pow(dist - D, 2);
				gradient[i] += 2.0 * (dist - D) * dist_grad;
			}
		}
	}


	void calcFeasibilityCost(const std::vector<Eigen::Vector3d>& q, double& cost,
											std::vector<Eigen::Vector3d>& gradient) 
	{
		cost = 0.0;
		int p=3;
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient.begin(), gradient.end(), zero);

		/* abbreviation */
		double  vm2, am2, ts_inv2, ts_inv4;
		vm2 = max_vel_ * max_vel_;
		am2 = max_acc_ * max_acc_;

		ts_inv2 = 1 / ts / ts;
		ts_inv4 = ts_inv2 * ts_inv2;

		Eigen::MatrixXd contorl_point(q.size(),3);

		for(int i=0;i<(int)q.size();i++)
		{
			contorl_point.row(i)=q[i];
		}
		trajectory_formation::NonUniformBspline bspline(contorl_point,3,ts);

		
		/* velocity feasibility */
		for (int i = 3; i < (int)q.size() - 1; i++) 
		{
			double tmp=p/(bspline.u_(i + bspline.p_ + 1) -bspline. u_(i + 1));
			Eigen::Vector3d v=(q[i+1]-q[i])*tmp;
			// for(int j=0;j<3;j++)
			// {
			// 	double vj=v(j);
			// 	if(vj*vj>vm2)
			// 	{
			// 		cost += pow(vj*vj-vm2, 2);
			// 		gradient[i + 0](j) += 2*vj*2*(vj*vj-vm2)*(-tmp);
			// 		gradient[i + 1](j) += 2*vj*2*(vj*vj-vm2)*(tmp);
			// 	}
			// }

			if(v.squaredNorm()>vm2)
			{
				cost += pow(v.squaredNorm()-vm2, 2);
				gradient[i + 0]+= 2*v*2*(v.squaredNorm()-vm2)*(-tmp);
				gradient[i + 1]+= 2*v*2*(v.squaredNorm()-vm2)*(tmp);
			}

		}

		/* acceleration feasibility */
		for (int i = 3; i <(int) q.size() - 2; i++) 
		{

			double tmp1=(p-1)/(bspline.u_(i + bspline.p_ + 1) -bspline. u_(i + 2));
			double tmp2=p/(bspline.u_(i + bspline.p_ + 2) -bspline. u_(i + 2));
			double tmp3=p/(bspline.u_(i + bspline.p_ + 1) -bspline. u_(i + 1));


			Eigen::Vector3d a=tmp1*(tmp2*(q[i+2]-q[i+1])-tmp3*(q[i+1]-q[i]));
			// for(int j=0;j<3;j++)
			// {
			// 	double aj=a(j);
			// 	if(aj*aj>am2)
			// 	{
			// 		cost += pow(aj*aj-am2, 2);
			// 		gradient[i + 0](j) += 2*a(j)*2*(aj*aj-am2)*tmp1*tmp3;
			// 		gradient[i + 1](j) +=2*a(j)*2*(aj*aj-am2)*tmp1* (tmp3-tmp1);
			// 		gradient[i + 2](j) +=2*a(j)*2*(aj*aj-am2)*tmp1*tmp2;
			// 	}
			// }
		
			if(a.squaredNorm()>am2)
			{
				cost += pow(a.squaredNorm()-am2, 2);
				gradient[i + 0] += 2*a*2*(a.squaredNorm()-am2)*tmp1*tmp3;
				gradient[i + 1] +=2*a*2*(a.squaredNorm()-am2)*tmp1* (tmp3-tmp1);
				gradient[i + 2] +=2*a*2*(a.squaredNorm()-am2)*tmp1*tmp2;
			}
		
		}
	}




	void calcFormationCost( std::vector<std::vector<Eigen::Vector3d>> q, double& cost,std::vector<std::vector<Eigen::Vector3d>>& gradient) 
	{
		cost=0;
		int order=3;
		int ctrl_point_num=q[0].size();
		Eigen::Vector3d zero(0, 0, 0);
		for(int i=0;i<gradient.size();i++)
		{
			std::fill(gradient[i].begin(), gradient[i].end(), zero);
		}

		int end_i;
		end_i=endPointSoftConstraint ? ctrl_point_num : (ctrl_point_num-order);


		for(int i=3;i<end_i;i++)
		{
			std::vector<Eigen::Vector3d> desired_formation=formation_structure_info.calculateAllPosition(q[0][i],q[1][i]);
			
			for(int id=2;id<agent_num;id++)
			{
				Eigen::Vector3d desired_p=desired_formation[id];
				cost+=(desired_p-q[id][i]).squaredNorm();

				gradient[id][i]+=-2*(desired_p-q[id][i]);

				gradient[0][i]+=(-formation_structure_info.trans_matrix[id]+Eigen::Matrix3d::Identity()).transpose()*2*(desired_p-q[id][i]);
				gradient[1][i]+=(formation_structure_info.trans_matrix[id]+Eigen::Matrix3d::Identity()).transpose()*2*(desired_p-q[id][i]);


			}

		}


	}

	void calcEndPointCost(const std::vector<Eigen::Vector3d>& q, double& cost,std::vector<Eigen::Vector3d>& gradient,Eigen::Vector3d end_pt_) 
	{

		cost = 0.0;
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient.begin(), gradient.end(), zero);

		Eigen::Vector3d end_q=q[q.size() - 1];

		cost += (end_q-end_pt_).squaredNorm();
		gradient[q.size() - 1] += 2 * (end_q-end_pt_);


		// cost = 0.0;
		// Eigen::Vector3d zero(0, 0, 0);
		// std::fill(gradient.begin(), gradient.end(), zero);

		// // zero cost and gradient in hard constraints
		// Eigen::Vector3d q_3, q_2, q_1, dq;
		// q_3 = q[q.size() - 3];
		// q_2 = q[q.size() - 2];
		// q_1 = q[q.size() - 1];

		// dq = 1 / 6.0 * (q_3 + 4 * q_2 + q_1) - end_pt_;
		// cost += dq.squaredNorm();

		// gradient[q.size() - 3] += 2 * dq * (1 / 6.0);
		// gradient[q.size() - 2] += 2 * dq * (4 / 6.0);
		// gradient[q.size() - 1] += 2 * dq * (1 / 6.0);

	}
	void calcHeightLimitationCost(const std::vector<Eigen::Vector3d>& q, double& cost,std::vector<Eigen::Vector3d>& gradient)
	{
		int order=3;
		cost = 0.0;
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient.begin(), gradient.end(), zero);

		for(int i=0;i<(int)q.size();i++)
		{
			cost+=pow(q[i](2)-setFlightHeight,2);
			gradient[i](2)=2*pow(q[i](2)-setFlightHeight,2);
		}
	}

	void calcvelocityDirectionCost(const std::vector<Eigen::Vector3d>& q, double& cost,std::vector<Eigen::Vector3d>& gradient,Eigen::Vector3d desired_v) 
	{
		cost = 0.0;
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient.begin(), gradient.end(), zero);
		int order=3;
		int p=3;

		Eigen::MatrixXd contorl_point(q.size(),3);
		for(int i=0;i<(int)q.size();i++)
		{
			contorl_point.row(i)=q[i];
		}
		trajectory_formation::NonUniformBspline bspline(contorl_point,3,ts);
		for (int i = order-1; i < (int)q.size() - 1; i++) 
		{
			double tmp=p/(bspline.u_(i + bspline.p_ + 1) -bspline. u_(i + 1));
			Eigen::Vector3d v=(q[i+1]-q[i])*tmp;
			double D=0.5;
			if(desired_v.transpose()*v<D)
			{
				cost+=-desired_v.transpose()*v+D;

				gradient[i + 0] += -desired_v*-tmp;
				gradient[i + 1] += -desired_v*tmp;
			}
		}
	}

	void calcCollisionCost( std::vector<Eigen::Vector3d> q_fisrt_id,  std::vector<Eigen::Vector3d> q_second_id,double& cost,
				std::vector<Eigen::Vector3d> gradient[2]) 
	{
		cost=0;
		int order=3;

		int ctrl_point_num=q_fisrt_id.size();
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient[0].begin(), gradient[0].end(), zero);
		std::fill(gradient[1].begin(), gradient[1].end(), zero);

		int end_i;
		end_i=endPointSoftConstraint ? ctrl_point_num : (ctrl_point_num-order);

		double collision_safety_distance=0.9;
		for(int i=3;i<end_i;i++)
		{
			double ratio=1;
			if(i>=ctrl_point_num*0.5)
				ratio*=lamda_SecondHalfCollisionPunishRatio;
			if((q_fisrt_id[i]-q_second_id[i]).squaredNorm()<collision_safety_distance*collision_safety_distance)
			{
				cost+=ratio*pow((q_fisrt_id[i]-q_second_id[i]).squaredNorm()-collision_safety_distance*collision_safety_distance,2);

				gradient[0][i]+=ratio*4*((q_fisrt_id[i]-q_second_id[i]).squaredNorm()-collision_safety_distance*collision_safety_distance)*(q_fisrt_id[i]-q_second_id[i]);
				gradient[1][i]+=-ratio*4*((q_fisrt_id[i]-q_second_id[i]).squaredNorm()-collision_safety_distance*collision_safety_distance)*(q_fisrt_id[i]-q_second_id[i]);

			}
		}

	}

	void calcBaseFormationCost(std::vector<Eigen::Vector3d> q0,  std::vector<Eigen::Vector3d> q1,double basep0p1,double& cost,
				std::vector<Eigen::Vector3d> gradient[2])
	{
		cost=0;
		int order=3;
		int ctrl_point_num=q0.size();
		Eigen::Vector3d zero(0, 0, 0);
		std::fill(gradient[0].begin(), gradient[0].end(), zero);
		std::fill(gradient[1].begin(), gradient[1].end(), zero);
		int end_i;
		end_i=endPointSoftConstraint ? ctrl_point_num : (ctrl_point_num-order);

		for(int i=3;i<end_i;i++)
		{
			cost+=std::pow((q0[i]-q1[i]).squaredNorm()-basep0p1*basep0p1,2);
			gradient[0][i]=2*((q0[i]-q1[i]).squaredNorm()-basep0p1*basep0p1)*2*(q0[i]-q1[i]);
			gradient[1][i]=2*((q0[i]-q1[i]).squaredNorm()-basep0p1*basep0p1)*2*(q1[i]-q0[i]);
		}	
		
		
	}


	double cost_J(const std::vector<double>& x, std::vector<double>& grad,double &cost) 
	{
		ros::Time tmp_time;
		// ros::Time cost_start_time=ros::Time::now();
		optimize_iter_times++;

		SmoothnessCost=0;
		DistanceCost=0;
		FeasibilityCost=0;
		CollisionCost=0;
		HeightLimitationCost=0;
		EndPointCost=0;
		velocityDirectionCost=0;
		cost=0;	
		FormationCost=0;
		SmoothnessCost_Sum=0;
		DistanceCost_Sum=0;
		FeasibilityCost_Sum=0;
		EndPointCost_Sum=0;
		velocityDirectionCost_Sum=0;
		CollisionCost_Sum=0;

		BaseFormationCost=0;

		// ROS_INFO("X SIZE: %ld",x.size());

		grad.resize(x.size());
		fill(grad.begin(), grad.end(), 0.0);


		//change data format
		int ctrl_p_num=init[0].getControlPointNum();


		// ROS_INFO("control point num: %d", ctrl_p_num);

		std::vector<std::vector<Eigen::Vector3d>> q(agent_num);

		getControlPointsFormOptimizationVariable(x,q);

		std::vector<std::vector<Eigen::Vector3d>> agent_gradient(agent_num);

		for(int i=0;i<agent_num;i++)
		{
			agent_gradient[i].resize(ctrl_p_num);
			std::fill(agent_gradient[i].begin(),agent_gradient[i].end(),Eigen::Vector3d::Zero());
		}

		for(int id=0;id<agent_num;id++)
		{
			std::vector<Eigen::Vector3d> tmp_gradient(ctrl_p_num,Eigen::Vector3d::Zero());

			std::vector<Eigen::Vector3d> SmoothnessGradient(ctrl_p_num);
			calcSmoothnessCost(q[id],  SmoothnessCost, SmoothnessGradient) ;
			SmoothnessCost*=lamda_Smoothness;	
			SmoothnessCost_Sum+=SmoothnessCost;

			std::vector<Eigen::Vector3d> DistanceGradient(ctrl_p_num);
			calcDistanceCost(q[id],  DistanceCost, DistanceGradient) ;
			DistanceCost*=lamda_Distance;
			DistanceCost_Sum+=DistanceCost;

			std::vector<Eigen::Vector3d> FeasibilityGradient(ctrl_p_num);
			calcFeasibilityCost(q[id],  FeasibilityCost, FeasibilityGradient) ;
			FeasibilityCost*=lamda_Feasibility;
			FeasibilityCost_Sum+=FeasibilityCost;
			
			cost+=(SmoothnessCost+DistanceCost+FeasibilityCost);


			for(int i=0;i<ctrl_p_num;i++)
			{
				agent_gradient[id][i]=lamda_Smoothness*SmoothnessGradient[i]+lamda_Distance*DistanceGradient[i]+lamda_Feasibility*FeasibilityGradient[i];
			}

			if(endPointSoftConstraint)
			{
				//EndPoint Cost
				std::vector<Eigen::Vector3d> EndPointGradient(ctrl_p_num);
				calcEndPointCost(q[id],  EndPointCost, EndPointGradient,init[id].control_points_.row(ctrl_p_num-1)) ;
				EndPointCost*=lamda_EndPoint;
				EndPointCost_Sum+=	EndPointCost;
				cost+=EndPointCost;	
				for(int i=0;i<ctrl_p_num;i++)
				{
					agent_gradient[id][i]+=lamda_EndPoint* EndPointGradient[i];
				}
			}

			if(velocityDirectionConstraint)
			{
				//velocityDirectionConstraint
				std::vector<Eigen::Vector3d> velocityDirectionGradient(ctrl_p_num);
				Eigen::Vector3d	desired_v_direction=(init[id].control_points_.row(init[id].n_ ).transpose()-trajectroyOptimize_current_p[id]).normalized();
				calcvelocityDirectionCost(q[id],  velocityDirectionCost, velocityDirectionGradient,desired_v_direction) ;
				velocityDirectionCost*=lamda_velocityDirectionCost	;
				velocityDirectionCost_Sum+=	velocityDirectionCost;
				cost+=velocityDirectionCost;	
				for(int i=0;i<ctrl_p_num;i++)
				{
					agent_gradient[id][i]+=lamda_velocityDirectionCost* velocityDirectionGradient[i];
				}
			}

		}
			// ROS_INFO("tmp2 spend time: %f",(tmp_time-cost_start_time).toSec());


		//Formation Cost
		std::vector<std::vector<Eigen::Vector3d>> FormationGradient(agent_num);
		for(int id=0;id<agent_num;id++)
		{
			FormationGradient[id].resize(ctrl_p_num);
		}
		calcFormationCost(q,  FormationCost, FormationGradient) ;
		FormationCost*=lamda_Formation;
		cost+=FormationCost;
		for(int i=0;i<ctrl_p_num;i++)
		{
			agent_gradient[0][i]+=lamda_Formation* FormationGradient[0][i];
			agent_gradient[1][i]+=lamda_Formation*FormationGradient[1][i];
			agent_gradient[2][i]+=lamda_Formation* FormationGradient[2][i];
			agent_gradient[3][i]+=lamda_Formation*FormationGradient[3][i];
		}


		//Colllision Cost
		for(int first_id=0;first_id<agent_num;first_id++)
		{
			for(int second_id=first_id+1;second_id<agent_num;second_id++)
			{
				std::vector<Eigen::Vector3d> CollisionGradient[2];
				CollisionGradient[0].resize(ctrl_p_num);
				CollisionGradient[1].resize(ctrl_p_num);
				calcCollisionCost(q[first_id],q[second_id]  ,CollisionCost, CollisionGradient) ;	
				CollisionCost*=lamda_Collision;
				CollisionCost_Sum+=CollisionCost;
				cost+=CollisionCost	;

				for(int i=0;i<ctrl_p_num;i++)
				{
					agent_gradient[first_id][i]+=lamda_Collision* CollisionGradient[0][i];
					agent_gradient[second_id][i]+=lamda_Collision*CollisionGradient[1][i];
				}

			}
		}		


		//BaseFormationCost
		std::vector<Eigen::Vector3d> BaseFormationGradient[2];
		BaseFormationGradient[0].resize(ctrl_p_num);
		BaseFormationGradient[1].resize(ctrl_p_num);
		calcBaseFormationCost(q[0],q[1],formation_base_edge_length  ,BaseFormationCost, BaseFormationGradient) ;	
		BaseFormationCost*=lamda_BaseFormation;
		cost+=BaseFormationCost;
		for(int i=0;i<ctrl_p_num;i++)
		{
			agent_gradient[0][i]+=lamda_BaseFormation* BaseFormationGradient[0][i];
			agent_gradient[1][i]+=lamda_BaseFormation*BaseFormationGradient[1][i];
		}


		//change gradient format
		for(int i=0;i<(int)x.size();i++)
		{
			int id=3*i/x_each_piece_num;
			if(i*3+2>=(int)x.size())break;
			grad[i*3]=agent_gradient[id][i%(x_each_piece_num/3)+3](0);
			grad[i*3+1]=agent_gradient[id][i%(x_each_piece_num/3)+3](1);
			grad[i*3+2]=agent_gradient[id][i%(x_each_piece_num/3)+3](2);
		}

		// ros::Time cost_end_time=ros::Time::now();

		// ROS_INFO("optimie cost: %f use: %f, smooth, distance, feasibility, FormationCost, Collision: %f %f %f %f %f",
		// cost,(cost_end_time-cost_start_time).toSec(),SmoothnessCost,DistanceCost,FeasibilityCost,FormationCost,CollisionCost);

		cost_record.push_back(cost);

		return cost;
	}

	static double costFunction(const std::vector<double>& x, std::vector<double>& grad, void* func_data)
	{
		FormationBsplineTrajectoryOptimize* opt = reinterpret_cast<FormationBsplineTrajectoryOptimize*>(func_data);
		double            cost;
		opt->cost_J(x, grad, cost);
  		return cost;
	}


	void reallocateTime(std::vector<trajectory_formation::NonUniformBspline>& sendBsplineTraj)
	{
		std::vector<trajectory_formation::NonUniformBspline> bspline_traj(agent_num);
		for(int id=0;id<agent_num;id++)
		{
			Eigen::MatrixXd optimize_ctrl_pts_matrix=Eigen::MatrixXd(ctrl_num,3);
			for(int i=0;i<ctrl_num;i++)
			{
				optimize_ctrl_pts_matrix.row(i)=optimize_ctrl_pts[id][i];
			}
			 bspline_traj[id].setUniformBspline(optimize_ctrl_pts_matrix,3,ts);
		}


		for(int i=0;i<ctrl_num-1;i++)
		{
			double v_max=-999;
			std::vector<Eigen::Vector3d> v(agent_num);
			int p=3;
			for(int id=0;id<agent_num;id++)
			{
				v[id]=p * (optimize_ctrl_pts[id][i +1] - optimize_ctrl_pts[id][i ] ) / (bspline_traj[id].u_(i + p + 1) -bspline_traj[id]. u_(i + 1));
				v_max=std::max(fabs(v[id](0)),(v_max));
				v_max=std::max(fabs(v[id](1)),(v_max));
			}
			double tmp_max_vel_=max_vel_*0.6;
			// ROS_INFO("v_max: %f",v_max);

			if(v_max>tmp_max_vel_)
			{
				for(int id=0;id<agent_num;id++)
				{
					double set_time_interval=(bspline_traj[id].u_(i + p + 1) -bspline_traj[id]. u_(i + 1))* v_max/tmp_max_vel_;
					bspline_traj[id].resetKnot(i + 1,i + p + 1,set_time_interval);
				}	
			}
	
		}
		

		for(int id=0;id<agent_num;id++)
		{
			// ROS_INFO("id: %d, time: %f //",agent_id,bspline_traj[id].getTimeSum());

			 bspline_traj[id].start_time=sendBsplineTraj[id].start_time;
			 sendBsplineTraj[id]=bspline_traj[id];
		}
		ROS_INFO("id: %d, RECOLLATE SUCCESS, traj time: %f//",agent_id,sendBsplineTraj[0].duration_time);


	}



	bool trajectroyOptimize(std::vector<Eigen::Vector3d> current_p,  std::vector<Eigen::Matrix<double, 9,1>>& optimize_traj,std::vector< Eigen::Vector3d>current_v,
							std::vector<trajectory_formation::NonUniformBspline>& sendBsplineTraj, double& trajcost)
	{

		// ROS_INFO("id: %d lamda_Smoothness: %f lamda_Distance:%f lamda_Feasibility:%f lamda_Formation:%f lamda_HeightLimitation:%f lamda_EndPoint:%f ,lamda_velocityDirectionCost:%f, lamda_SecondHalfCollisionPunishRatio:%f",
		// 		agent_id,lamda_Smoothness,lamda_Distance,lamda_Feasibility, lamda_Formation,
		// 		  lamda_HeightLimitation,lamda_EndPoint,
		// 		 lamda_velocityDirectionCost, lamda_SecondHalfCollisionPunishRatio);

		// for(int id=0;id<agent_num;id++)
		// {
		// 	ROS_INFO("id: %d get id: %d current_p: %f %f %f current_v: %f %f %f",agent_id,id,current_p[id](0),current_p[id](1),current_p[id](2),
		// 	current_v[id](0),current_v[id](1),current_v[id](2));	
		// }

		std::vector<int> TrajTotalPointNum(agent_num);
		std::vector<Eigen::MatrixXd> ctrl_pts(agent_num);
		std::vector<Eigen::MatrixXd> posList(agent_num);
		optimize_traj.clear();

		for(int id=0;id<agent_num;id++)
		{
			std::vector<Eigen::Vector3i> check_idx;
			for(int i=0;i<(int)agent_idxList[id].size()-1;i++)
			{
				if(agent_idxList[id][i]!=agent_idxList[id][i+1])
					check_idx.push_back(agent_idxList[id][i]);
			}
			check_idx.push_back(agent_idxList[id].back());
			agent_idxList[id]=check_idx;

			trajectroyOptimize_current_p[id]=current_p[id];
			posList[id]=Eigen::MatrixXd(agent_idxList[id].size(),3);

			for(int i=0;i<(int)agent_idxList[id].size()-1;i++)
			{
				if(i==0) 
					posList[id].row(i)=current_p[id];
				else
					posList[id].row(i)=bsplineOptimizeMapPtr->getMapGlobalPosfromLocalIdx(agent_idxList[id][i]);
				posList[id].row(i)(2)=setFlightHeight;
				// ROS_INFO("index: %d %d %d pos: %f %f %f",agent_idxList[id][i](0),agent_idxList[id][i](1),agent_idxList[id][i](2),
				// 		posList[id](i,0),posList[id](i,1),posList[id](i,2));
			}

			posList[id].row(agent_idxList[id].size()-1)=bsplineOptimizeMapPtr->getMapGlobalPosfromLocalIdx(agent_idxList[id].back());
			posList[id].row(agent_idxList[id].size()-1)(2)=setFlightHeight;
		}

		// for(int i=0;i<posList[0].rows();i++)
		// {
		// 	ROS_INFO("id: %d pose: %f %f %f",agent_id,posList[0](i,0),posList[0](i,1),posList[0](i,2));
		// }


		// std::vector<double> start_end_va = {0,0,0,0,0,0};

		for(int id=0;id<agent_num;id++)
		{
			std::vector<double> start_end_va = {current_v[id](0),current_v[id](1),0,0,plan_end_v,0,0,0};
			// std::vector<double> start_end_va = {0,0,0,0,0,0,0,0};
			getMinimum_X_PlanTrajTotalPointNum(posList[id],1/ts,start_end_va,max_vel_*0.8,max_acc_*0.8,1,TrajTotalPointNum[id]);
		}

		int average_TrajTotalPointNum=std::accumulate(TrajTotalPointNum.begin() , TrajTotalPointNum.end() , 0)/agent_num;

		// ROS_INFO("id: %d traj total time: %f",agent_id,(average_TrajTotalPointNum-1)*ts);

		for(int id=0;id<agent_num;id++)
		{
			Eigen::Vector3d tmp_v(plan_end_v,0,0);

			std::vector<double> start_end_va = {current_v[id](0),current_v[id](1),0,0,plan_end_v,0,0,0};
			// std::vector<double> start_end_va = {0,0,0,0,0,0,0,0};  //v0x,v0y,a0x,a0y, vfx,vfy,afx,afy
			std::vector<Eigen::Vector3d> minimum_snap_pathList;
			// if(agent_id==2)
			// {
			// 	std::cout<<"minimum_snap_pathList: "<<std::endl;
			// 	for(int i=0;i<average_TrajTotalPointNum;i++)
			// 	{
			// 		std::cout<<minimum_snap_pathList[id](0)<<", "<<minimum_snap_pathList[id](1)<<", "<<minimum_snap_pathList[id](2)<<", "<<std::endl;
			// 	}
			// }			
			minimum_snap_pathList=Minimum_X_Plan(posList[id],1/ts,start_end_va,max_vel_*0.8,max_acc_*0.8,1,average_TrajTotalPointNum);
			// minimum_snap_pathList=Navie_Plan(posList[id],1/ts,start_end_va,max_vel_*0.9,max_acc_*0.9,1,average_TrajTotalPointNum);

			std::vector<Eigen::Vector3d>  start_end_derivatives;
			start_end_derivatives.push_back(current_v[id]);
			start_end_derivatives.push_back(Eigen::Vector3d::Zero());
			// start_end_derivatives.push_back(Eigen::Vector3d::Zero());
			start_end_derivatives.push_back(tmp_v);
			// start_end_derivatives.push_back(Eigen::Vector3d::Zero());
			start_end_derivatives.push_back(Eigen::Vector3d::Zero());

			// if(agent_id==2)
			// {
			// 	std::cout<<"minimum_snap_pathList: "<<std::endl;
			// 	for(int i=0;i<average_TrajTotalPointNum;i++)
			// 	{
			// 		std::cout<<minimum_snap_pathList[id](0)<<", "<<minimum_snap_pathList[id](1)<<", "<<minimum_snap_pathList[id](2)<<", "<<std::endl;
			// 	}
			// }
			
			trajectory_formation::parameterizeToBspline(ts, minimum_snap_pathList, start_end_derivatives, ctrl_pts[id]);
			// ROS_INFO("id :%d, control points num: %ld",id,ctrl_pts[id].rows());
			// std::cout<<"ctrl_pts: \n"<<ctrl_pts[id]<<std::endl;
			// std::cout<<"ctrl_pts rows: "<<ctrl_pts.rows()<<std::endl;
			init[id].setUniformBspline(ctrl_pts[id], 3, ts);


			// std::cout<<"id "<<id<<" ctrl_pts[id]:\n"<<ctrl_pts[id]<<std::endl;
		}
		bsplineDurationTime=init[0].getTimeSum();
		bsplineBasisValueAndStartIdx.clear();
		for(int i=0;;i++)
		{
			double t_cur=i*0.1;
			if(t_cur>init[0].getTimeSum())break;
			std::vector<double> basevalue;
			int start_idx;
			init[0].getBaseValue(t_cur,basevalue,start_idx);
			std::pair<int,std::vector<double>> pair;
			pair.first=start_idx;
			pair.second=basevalue;
			bsplineBasisValueAndStartIdx.push_back(pair);
		}	


		ctrl_num=init[0].getControlPointNum();
		if(endPointSoftConstraint)
			variable_num=agent_num*3*(ctrl_num-3);
		else
			variable_num=agent_num*3*(ctrl_num-6);

		if(endPointSoftConstraint)
			x_each_piece_num=3*(ctrl_num-3);
		else
			x_each_piece_num=3*(ctrl_num-6);
		// std::vector<double> x(2*3*(ctrl_num-6),0);

		// cost_J(x,grad,func_data);

		ROS_INFO("\033[1;%dm id:%d variable_num: %d ctrl point num: %d \033[0m",color_id,agent_id,variable_num,ctrl_num);

		// ROS_INFO("start optimization/////////////////");


		std::vector<double> q(variable_num,0);
		std::vector<double >lower_bound(variable_num,0);
		std::vector<double >upper_bound(variable_num,0);

		double bound=5;
		double z_bound=0.01;
		for(int i=0;i<variable_num;i++)
		{
			int id=3*i/x_each_piece_num;
			if(i*3+2>=variable_num)break;
			// ROS_INFO("i: %d index: %d id: %d",i,i%(x_each_piece_num/3)+3,id);

			q[i*3]=ctrl_pts[id].row(i%(x_each_piece_num/3)+3)(0);
			q[i*3+1]=ctrl_pts[id].row(i%(x_each_piece_num/3)+3)(1);
			q[i*3+2]=ctrl_pts[id].row(i%(x_each_piece_num/3)+3)(2);

			lower_bound[i*3]=q[i*3]-bound;
			lower_bound[i*3+1]=q[i*3+1]-bound;
			lower_bound[i*3+2]=0.09;

			upper_bound[i*3]=q[i*3]+bound;
			upper_bound[i*3+1]=q[i*3+1]+bound;
			upper_bound[i*3+2]=0.11;

		}

		nlopt::opt opt(nlopt::algorithm(11), variable_num);
		unsigned int M=12;
		opt.set_vector_storage( M);
		opt.set_min_objective(costFunction,this);
		// opt.set_maxeval(160);
		opt.set_maxtime(0.030);
		opt.set_xtol_rel(1e-4);

		opt.set_lower_bounds(lower_bound);
		opt.set_upper_bounds(upper_bound);

		optimize_iter_times=0;
		cost_record.clear();
		bool trajectory_success=true;
		ros::Time optimization_start_time=ros::Time::now();
		double        final_cost;
		nlopt::result result;
		try {

			result= opt.optimize(q, final_cost);
			trajectory_success=true;
			optimize_round++;
		} catch (std::exception& e) {
			ROS_WARN("agentid: %d [Optimization]: nlopt exception ",agent_id);
			trajectory_success=false;
			cout <<"id: "<<agent_id<<" "<< e.what() << endl;
			ROS_ERROR("agentid: %d OPTIMIZTAION ERROR !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!",agent_id);
		}
		
		ros::Time optimization_end_time=ros::Time::now();
		trajcost=final_cost;
		ROS_INFO("\033[1;%dm id:%d optimize success: %d, optimize_iter_times: %d optimization use time: %f final cost: %f round: %d\033[0m",color_id,agent_id ,trajectory_success,
							optimize_iter_times,(optimization_end_time-optimization_start_time).toSec(),final_cost,optimize_round);
		ROS_INFO("\033[1;%dm id:%d Smoothness: %f DistanceCost: %f Feasibility: %f formation: %f EndPointCost: %f velocitydirection: %f COLLISION:%f BaseFormationCost:%f\033[0m",
						color_id,agent_id,SmoothnessCost_Sum,DistanceCost_Sum,FeasibilityCost_Sum,CollisionCost,EndPointCost_Sum,velocityDirectionCost_Sum ,
							CollisionCost_Sum,BaseFormationCost);
		if(trajectory_success)
		{
			getControlPointsFormOptimizationVariable(q,optimize_ctrl_pts);

			reallocateTime( sendBsplineTraj);


			for(int id=0;id<agent_num;id++)
			{

				trajectory_formation::NonUniformBspline bspline_traj_vel=sendBsplineTraj[id].getDerivative();
				trajectory_formation::NonUniformBspline bspline_traj_acc=bspline_traj_vel.getDerivative();
				
				// ROS_INFO("ID: %d sendBsplineTraj[id].getTimeSum(): %f",agent_id,sendBsplineTraj[id].getTimeSum());

				for(int i=0;;i++)
				{
					double t_cur=i*1.0/control_frequency;
					if(t_cur>sendBsplineTraj[id].getTimeSum())break;
					Eigen::Vector3d pos = sendBsplineTraj[id].evaluateDeBoorT(t_cur);
					Eigen::Vector3d vel=bspline_traj_vel.evaluateDeBoorT(t_cur);
					Eigen::Vector3d acc=bspline_traj_acc.evaluateDeBoorT(t_cur);

					Eigen::Matrix<double , 9,1> traj_point;
					traj_point.head(3)=pos;
					traj_point.block(3,0,3,1)=vel;
					traj_point.tail(3)=acc;
					optimize_traj.push_back(traj_point);
				}	

			}






		}
		return trajectory_success;

	}


};





#endif